from enum import auto
from socket import *
from car2 import *
import json
import numpy as np
import cv2
from opengar import *
from http.server import BaseHTTPRequestHandler, HTTPServer
import urllib
auto = False
GA = garsorting()

def lockrp(ges,tem):
    for temp in tem:
        center = (temp[1]+temp[3]+ges[1]+ges[3])/4    #left - right
        if center > temp[1] and center < temp[3]:
            tar = temp
            return tar
    return ges


def getimg():                               #get image
    img_path='/home/pi/Desktop/rk3399'
    flag,img=cv2.VideoCapture(0).read()
    cv2.imwrite(os.path.join(img_path, "new.jpg"),img)


class autoThread(threading.Thread):
    def __init__(self, audio = False, Fist = False, startgar = False):
        threading.Thread.__init__(self)
        self.audio = audio
        self.Fist = Fist
        self.startgar = startgar
        
    def run(self):
        HOST = "10.181.250.217"      #host of rk3399
        PORT1 = 3399
        d = 0
        audio = self.audio
        Fist = self.Fist
        mode = 'waiting'
        tork = socket(AF_INET, SOCK_STREAM)
        tork.connect((HOST,PORT1))
        print ('raspberry pi get the rkpro')
        
        if audio == True:
            tork.send('audio'.encode())
        else:
            tork.send('wake'.encode())
        
        rkdata = tork.recv(1024).decode('utf-8')    #wait to wake up
        mode = rkdata

        if mode == 'wake' :
            if self.startgar == False:
                mode = 'detectionmode'
            else:
                mode = 'garbagemode'
                d = 1
            data = mode.encode()
            tork.send(data)                         #send 'detectionmode' to start the new mode
        else :
            print(mode+' Error')
        
        while d == 0 :
            getimg()
            data = 'detect'
            tork.send(data.encode())                 #send 'detect' to detect obj
            rkdata = tork.recv(2048).decode('utf-8')    #receive the result
            obj = json.loads(rkdata)
            #print(obj)
            l = len(obj)
            l = l//5
            if l != 0:
                obj = np.array(obj)
                obj.shape = (l, 5)
                temp = findtar(obj)         #check whether there is a person in the photo
            else :
                temp = 'notar'
            
            print(temp)

            if temp != 'notar' and Fist == True:        #gesture detect
                data = 'peoplefind'
                tork.send(data.encode())
                rkdata = tork.recv(2048).decode('utf-8')    #receive the gesture result
                gestu = json.loads(rkdata)
                if gestu[0] == -1:
                    print('getfist')
                    tar = lockrp(gestu,temp)
                    d = automove(tar)                              #move to the target
                else:
                    print('nofist')
                    d = automove('notar')
            
            elif temp != 'notar' and Fist == False:
                d = automove(temp[0])                              #move to the target
            else:
                d = automove('notar')
        if mode == 'detectionmode':
            mode = "garbagemode"
            data = mode.encode()
            tork.send(data)                    #send 'garbage' to start sorting
        
        #rkdata = tork.recv(2048).decode('utf-8')    #waiting for sorting
        #print(rkdata)
        time.sleep(2)
        GA.sort()
        gar = GA.result
        opengar(gar)                               #open the corresponding trashbin
        tork.close()
        global auto
        auto = False


def run_app():
    
    HOST ='10.181.250.226'     #host of rpi
    PORT = 8888
    sapp = socket(AF_INET, SOCK_STREAM)
    sapp.bind((HOST, PORT))
    sapp.listen(10)
    print ('raspberry pi is listening on port '+ str(PORT))
    while True:
        try:
            global auto
            auto = False
            connapp, addrapp = sapp.accept()
            print ('Connected by:', addrapp)
            while True:
                message = connapp.recv(1024).decode('utf-8').replace('\n','')
                if not message:
                    break
                print(message)
                
                if auto == True:
                    stop_thread(A)
                    Car.stop()
                    auto = False
                if message == 'auto':
                    A=autoThread(audio = False, Fist = False, startgar = False)
                    A.start()
                    auto = True 
                elif message == 'auto_audio':
                    A=autoThread(audio = True, Fist = False, startgar = False)
                    A.start()
                    auto = True 
                elif message == 'auto_Fist':
                    A=autoThread(audio = False, Fist = True, startgar = False)
                    A.start()
                    auto = True 
                elif message == 'auto_both':
                    A=autoThread(audio = True, Fist = True, startgar = False)
                    A.start()
                    auto = True 
                elif message == 'garbage':
                    A=autoThread(audio = False, Fist = False, startgar = True)
                    A.start()
                elif message == 'forward':
                    Car.forward()
                elif message == 'back':
                    Car.back()
                elif message == 'left':
                    Car.left()
                elif message == 'right':
                    Car.right()
                else:
                    Car.stop()

            connapp.close()
        
        except:
            print('Error')
            continue 

Auto1=autoThread(audio = False, Fist = False, startgar = False)
Auto2=autoThread(audio = True, Fist = False, startgar = False)
Auto3=autoThread(audio = False, Fist = True, startgar = False)  
Auto4=autoThread(audio = True, Fist = True, startgar = False)
Auto5=autoThread(audio = False, Fist = False, startgar = True)   
#automap = [Auto1, Auto2, Auto3, Auto4, Auto5]
automap = [ autoThread(audio = False, Fist = False, startgar = False),\
            autoThread(audio = True, Fist = False, startgar = False), \
            autoThread(audio = False, Fist = True, startgar = False), \
            autoThread(audio = True, Fist = True, startgar = False), \
            autoThread(audio = False, Fist = False, startgar = True)]
autohttp = -1
class CarServer(BaseHTTPRequestHandler):
    
    def get_host_ip(self):
        '''
        This method is used for getting local ip address
        The car server will deploy on this ip
        '''
        try:
            serverSocket = socket(AF_INET, SOCK_DGRAM)
            serverSocket.connect(("8.8.8.8", 80))
            localIP = serverSocket.getsockname()[0]
        finally:
            return localIP
 
    def do_GET(self):
        '''
        Define the car control GUI for client
        For the first edition, it will only return direction contol GUI
        '''
        localIP = CarServer.get_host_ip(self)
 
        # Read control page html file from control.html
        controlPageFile = open("./control.html")
        controlPageGUI = controlPageFile.read()
        controlPageFile.close()
        controlPageGUI = controlPageGUI.replace(
            "requestAddress", "http://" + localIP + ":9090/")
        controlPageGUI = controlPageGUI.replace(
            "cameraAddress", "http://" + localIP + ":8080/")
 
        self.send_response(200)
        self.send_header("Content-type", "text/html")
        self.end_headers()
        self.wfile.write(controlPageGUI.encode())
 
    def do_POST(self):
        length = int(self.headers['Content-Length'])
        qs = self.rfile.read(length)
        direction = qs.decode()
        #print(direction)
        message = direction
        print(message)
        Car.stop()
        global autohttp, Auto1, Auto2, Auto3, Auto4, Auto5, automap, auto        
        if autohttp != -1 and auto == True:
            stop_thread(automap[autohttp])
            auto = False
            Car.stop()
            autohttp = -1
        
        automap = [ autoThread(audio = False, Fist = False, startgar = False),\
                    autoThread(audio = True, Fist = False, startgar = False), \
                    autoThread(audio = False, Fist = True, startgar = False), \
                    autoThread(audio = True, Fist = True, startgar = False), \
                    autoThread(audio = False, Fist = False, startgar = True)]
            
        if message == 'auto' :
            automap[0].start()
            autohttp = 0   
            auto = True 
        elif message == 'auto_audio':
            automap[1].start()
            autohttp = 1  
            auto = True 
        elif message == 'auto_Fist':
            automap[2].start()
            autohttp = 2       
            auto = True 
        elif message == 'auto_both':
            automap[3].start()
            autohttp = 3
            auto = True 
        elif message == 'garbage':
            automap[4].start()
            autohttp = 4
            auto = True 
        elif message == 'forward':
            Car.forward()
        elif message == 'back':
            Car.back()
        elif message == 'left':
            Car.left()
        elif message == 'right':
            Car.right()
        else:
            Car.stop()

        self.send_response(200)




if __name__ == '__main__':
    
    raspCarServer = CarServer
    hostIP = raspCarServer.get_host_ip(raspCarServer)
    hostPort = 9090
    myServer = HTTPServer((hostIP, hostPort), raspCarServer)
 
    print(time.asctime(), "Server Starts - %s:%s" % (hostIP, hostPort))
 
    try:
        myServer.serve_forever()
    except KeyboardInterrupt:
        pass
    
    #run_app()
